#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include "Vector3.h"

/* compile with -lpthread and -llo */

#include <pthread.h>
#include "lo/lo.h"

#define UPDATE_PATH "/pfs/listener/0001/updateAxes"
#define INFINITY_COMMAND "/infinity/target"
#define PLC_NUMBER 7
#define LISTEN_PORT "25669"
#define LOCAL_PORT "25670"
#define NUM_AXES 8

void * target_reporter( void * arg);

int emergencystop = 0;

int speed = 0;
int goal = 0;

lo_address t;
lo_address ipad;

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

int axis_position[NUM_AXES] = { 1 };
int axis_status[NUM_AXES] = { 0 };
int axis_error[NUM_AXES] = { 0 };
int axis_number[NUM_AXES] = { 0 };
int axis_load[NUM_AXES] = { 0 };



lo_address local_broadcast;

int position = 0;
int target = 0;

int min = 3686;
int axis_target = 3686;

int max = 9002;

int sn = 0;

int updatecount = 0;
int targetcount = 0;

void error(int num, const char *m, const char *path);

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int min_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int main() {
	pthread_t tid[1];
	int rc;

	t = lo_address_new("192.168.2.57", "25666");
        ipad = lo_address_new("192.168.2.69", "1025");

	rc = pthread_create( &tid[0], NULL, target_reporter, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}


	local_broadcast = lo_address_new("192.168.2.89", LOCAL_PORT);

	/* start a new server on port LISTEN_PORT */
	lo_server_thread st = lo_server_thread_new(LISTEN_PORT, error);

	/* add function to catch UPDATE_PATH OSC commands */
	lo_server_thread_add_method(st, UPDATE_PATH, NULL, update_handler, NULL);

	lo_server_thread_add_method(st, INFINITY_COMMAND, "ii", target_handler, NULL);

	lo_server_thread_add_method(st, "/stop", NULL, emergency_handler, NULL);

	lo_server_thread_add_method(st, "/state", "i", state_handler, NULL);

	lo_server_thread_add_method(st, "/playback", "i", playback_handler, NULL);

        lo_server_thread_add_method(st, "/min", "i", min_handler, NULL);

	lo_server_thread_start(st);

	while (1) {
		//Polling Loop
		usleep(1000000);

		int i;
		if(updatecount == 0){
			printf("NO UPDATES!\n");
		}
		else{
			pthread_mutex_lock(&mutex);
			updatecount = 0;
			targetcount = 0;
			for(i = 0; i < NUM_AXES; i++){
				printf("Axis: %d Pos: %d Load: %d Status: %d Error: %d\n", axis_number[i], axis_position[i], axis_load[i], axis_status[i], axis_error[i]);
      			}
			printf("Position: %d (%d percent) Target: %d Speed: %d Goal: %d\n", position, (int)((double)position/((double)max-(double)min)*100.0), target, (int)(((double)speed/100.0)*10), goal);
			pthread_mutex_unlock(&mutex);
		}
	}

	lo_server_thread_free(st);

	return 0;
}

void loadstate(int state){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "ii", sn, state);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/pfs/ur/0006/loadState", message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void startplayback(char* path){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "i", sn);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void sendtoposition(char* path, int axis, int position){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "iii", sn, axis, position);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void setspeed(char* path, int speed){
    //sn++;
    if(sn > 65535)
      sn = 0;
    lo_message message = lo_message_new();
    lo_message_add(message, "ii", sn, speed);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, path, message); 
    lo_send_bundle(t, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
}

void error(int num, const char *msg, const char *path)
{
	printf("liblo server error %d in path %s: %s\n", num, path, msg);
	fflush(stdout);
}

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	int PLC = argv[0]->i;
	if(PLC != PLC_NUMBER)
		return 1;
	int i;
	pthread_mutex_lock(&mutex);
	for(i = 0; i < NUM_AXES; i++){
		axis_number[i] = argv[(i*5)+2]->i;
		axis_position[i] = argv[(i*5)+3]->i;
		axis_load[i] = argv[(i*5)+4]->i;
		axis_status[i] = argv[(i*5)+5]->i;
		axis_error[i] = argv[(i*5)+6]->i;
	}
	position = axis_position[4] - min;
	updatecount++;
	pthread_mutex_unlock(&mutex);
	if (lo_send(local_broadcast, "/infinity/update", "i", position) == -1) {
			printf("OSC error %d: %s\n", lo_address_errno(local_broadcast), lo_address_errstr(local_broadcast));
		}    
        lo_send(local_broadcast, "/diff", "i", min);
        if(axis_position[4] == min )
          lo_send(ipad, "/min", "i", 1);
        else
          lo_send(ipad, "/min", "i", 0);
	fflush(stdout);

	return 1;
}

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		void *data, void *user_data){
	pthread_mutex_lock(&mutex);
	target = argv[0]->i;
	if(target > 5351)
		target = 5351;
	if(target < 0)
		target = 0;
	axis_target = target + min;
	speed = argv[1]->i;
	targetcount++;

	pthread_mutex_unlock(&mutex);
	return 1;  
}

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	printf("GOT!\n");
	pthread_mutex_lock(&mutex);
	while(1){
		printf("EMERGENCY STOP!\n");
		setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
		sendtoposition("/pfs/ur/0006/pb/1/axis/0005/setposition", 5, axis_position[4]);
		usleep(50000);
	}
}

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	pthread_mutex_lock(&mutex);
	printf("STATE LOADING\n");
	loadstate(argv[0]->i);
	pthread_mutex_unlock(&mutex);
}

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	pthread_mutex_lock(&mutex);
	if(argv[0]->i<1 || argv[0]->i > 6)
		return 0;
	char str[100];
	printf("PLAYBACK %d\n",argv[0]->i);
	sprintf(str, "/pfs/ur/0006/pb/%d/start", argv[0]->i);
	startplayback(str);
	pthread_mutex_unlock(&mutex);
}

void * target_reporter(void * arg){
  while(1){
    pthread_mutex_lock(&mutex);
    if(axis_target < min)
      axis_target = min;
    if(axis_target > max)
      axis_target = max;
    if(axis_target >= min && axis_target <= max){ 
        goal = speed;
        if(axis_target < 5000)
          goal = speed / 3;
        int speedout = (int)(((double)speed/100.0)*15);
        if(speedout < 0)
          speedout = 0;
        if(speedout > 10)
          speedout = 10;
        setspeed("/pfs/ur/0006/pb/1/setSpeed", speedout);
        sendtoposition("/pfs/ur/0006/pb/1/axis/0005/setposition", 5, axis_target);
    }
    else{
      //printf("OUT OF RANGE\n");
    }
    pthread_mutex_unlock(&mutex);
    usleep(50000);
  }
}

int min_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  min = axis_position[4];
  pthread_mutex_unlock(&mutex);
}

